package it.unibo.refolding.pico;

import it.unibo.refolding.common.CommonThings;
import it.unibo.refolding.pico.PicoDataAnalyser.ContactPointsAndSlope;
import it.unibo.refolding.pico.PicoDataAnalyser.PeakAnalysisInput;

import java.util.logging.Level;
import java.util.logging.Logger;

public class ApproachRetractionPush {
	public final double approachPushingFromDeflectionM;
	public final double retractionPushingM;

	private static final Logger logger = Logger
		.getLogger(ApproachRetractionPush.class.getSimpleName());

	public ApproachRetractionPush(double approachPushingFromDeflectionM, double retractionPushingM) {
		this.approachPushingFromDeflectionM = approachPushingFromDeflectionM;
		this.retractionPushingM = retractionPushingM;
	}

	public static ApproachRetractionPush computeApproachRetractionPush(
		PeakAnalysisInput peakAnalysisInput) {
		return computeApproachRetractionPush(
			peakAnalysisInput.contactPointsAndSlope, peakAnalysisInput.plainPicoData);
	}

	public static ApproachRetractionPush computeApproachRetractionPush(
		ContactPointsAndSlope contactPointsAndSlope, PlainPicoData plainPicoData) {
		double approachPushingFromDeflectionM = ApproachRetractionAssistant
			.approachPushingDistanceZLsb(
				plainPicoData.approach, contactPointsAndSlope.approachContactPointAndSlope()) * plainPicoData.deflectionLsbToMetresFactor();
		int approachArgMax = CommonThings.argMax(plainPicoData.sharedRetraction().sharedZArray());
		if (approachArgMax > plainPicoData.sharedRetraction().timeInstantCount() / 20) {
			logger.log(Level.WARNING, "ArgMax(Z)={0} should be close to zero instead.", approachArgMax);
		}
		double approachPushingM = (plainPicoData.sharedRetraction().z(approachArgMax) - contactPointsAndSlope.approachContactPoint.zLsb)
			* plainPicoData.zLsbToMetresFactor;
		if (logger.isLoggable(Level.FINER)) {
      logger.log(
        Level.FINER, "approachArgMax={0} approachPushingNm={1} approachPushingFromDeflectionNm={2}",
        new Object[] {approachArgMax, approachPushingM * 1e9, approachPushingFromDeflectionM * 1e9});
		}
		// Approach and Retraction both start from the higher piezo position.
		double retractionPushingFromDeflectionM = ApproachRetractionAssistant
			.approachPushingDistanceZLsb(
				plainPicoData.retraction, contactPointsAndSlope.retractionContactPointAndSlope()) * plainPicoData.deflectionLsbToMetresFactor();
		int retractionArgMax = CommonThings.argMax(plainPicoData.sharedRetraction().sharedZArray());
		if (retractionArgMax > plainPicoData.sharedRetraction().timeInstantCount() / 20) {
			logger.log(Level.WARNING, "ArgMax(Z)={0} should be close to zero instead.", retractionArgMax);
		}
		double retractionPushingM = (plainPicoData.sharedRetraction().z(approachArgMax) - contactPointsAndSlope.retractionContactPoint.zLsb)
			* plainPicoData.zLsbToMetresFactor;
    if (logger.isLoggable(Level.FINER)) {
      logger.log(
        Level.FINER,
        "retractionArgMax={0} retractionPushingNm={1} retractionPushingFromDeflectionNm={2}",
        new Object[] {
        retractionArgMax, retractionPushingM * 1e9, retractionPushingFromDeflectionM * 1e9});
    }
		ApproachRetractionPush approachRetractionPush = new ApproachRetractionPush(
			approachPushingFromDeflectionM, retractionPushingFromDeflectionM);
		return approachRetractionPush;
	}
}
